import sensor, image, time
from pyb import Pin

# 红色阈值
red_threshold =(40, 300, 20, 84, 0, 70)
# 绿色阈值
green_threshold = (200, 1000, 500, -28, -22, 67)


# 颜色1: 红色的颜色代码
red_color_code = 1 # code = 2^0 = 1
# 颜色2: 绿色的颜色代码
green_color_code = 2 # code = 2^1 = 2

sensor.reset() # 初始化摄像头
sensor.set_pixformat(sensor.RGB565) # 选择像素模式 RGB565.
sensor.set_framesize(sensor.QQVGA)
sensor.skip_frames(10)
sensor.set_auto_whitebal(False) #关闭白平衡
clock = time.clock()

while(True):
    clock.tick()
    img = sensor.snapshot() # 拍照，返回图像

    blobs = img.find_blobs([red_threshold, green_threshold], area_threshold=100)
    if blobs:

        for blob in blobs:
            x = blob[0]
            y = blob[1]
            width = blob[2]
            height = blob[3]
            center_x = blob[5]
            center_y = blob[6]

            color_code = blob[8] # 颜色代码
            if color_code == red_color_code:
                img.draw_string(x, y - 10, "red", color = (0xFF, 0x00, 0x00))
            elif color_code == green_color_code:
                img.draw_string(x, y - 10, "green", color = (0x00, 0xFF, 0x00))


            img.draw_rectangle([x, y, width, height])

            img.draw_cross(center_x, center_y)
    pin1 = Pin('P1', Pin.OUT_PP, Pin.PULL_NONE)

    if(blobs):
        pin1.value(0)
    else:
        pin1.value(1)

    print(clock.fps())
